# Hello World Example
#
# Welcome to the MaixPy IDE!
# 1. Conenct board to computer
# 2. Select board at the top of MaixPy IDE: `tools->Select Board`
# 3. Click the connect buttion below to connect board
# 4. Click on the green run arrow button below to run the script!

import sensor, image, time, lcd
from machine import Timer, UART
from fpioa_manager import fm
lcd.init(freq=15000000, invert=0)
lcd.rotation(2)

sensor.reset()                      # Reset and initialize the sensor. It will
                                    # run automatically, call sensor.run(0) to stop
fm.register(7, fm.fpioa.UART1_RX, force = True)             # 配置 7 脚为 UART1_RX 强制注册
fm.register(8, fm.fpioa.UART1_TX, force = True)             # 配置 8 脚为 UART1_TX 强制注册

uart = UART(UART.UART1, 115200, 8, 0, 1)
sensor.set_hmirror(0)
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA)   # Set frame size to QVGA (320x240)
sensor.run(1)
#以上为系统拍照初始化参数

dx = 20
dy = 5
#上下左右裁边
area = (dx,dy,160-dx*2,120-dy*2)
osd = [0,0,0,0,0,0,0,0,0,0,0,0]
clock = time.clock()                # Create a clock object to track the FPS.
temp = b'r'
img = sensor.snapshot()
while(True):
	clock.tick()                    # Update the FPS clock.
	#接收上位机发来的程序分支字符
	
	if uart.any():
		tempc = uart.read(1)
		if tempc != b'\00':
			temp = tempc

	if temp == b't': #测试分支字符
		uart.write("OK")
	if temp == b'h': #红点追踪初始化
		sensor.set_pixformat(sensor.RGB565)
		sensor.set_framesize(sensor.QVGA)
		sensor.set_auto_gain(False) # must be turned off for color tracking
		sensor.set_auto_whitebal(False) # must be turned off for color tracking
		sensor.skip_frames(time = 1000)     # Wait for settings take effect.
		print("init_once")
		temp = b'i'

	if temp == b'i':   #红点追踪循环
		print("red_trace_loop")
		img=sensor.snapshot()
		blobs = img.find_blobs([(0, 100, -128, -30, -128, 127)], pixels_threshold=20, area_threshold=20,merge = True)
		if blobs:
			for b in blobs:
				tmp=img.draw_cross(b[5], b[6])
				print("L[%d,%d]" % (b[5],b[6]))
				uart.write("L%03d,%03d" % (b[5],b[6]))
				osd[10] = b[5]
				osd[11] = b[6]
		blobs = img.find_blobs([(0, 100, 35, 127, -128, 127)], pixels_threshold=10 , area_threshold=10) #红色阈值以及光斑尺寸下限
		if blobs:
			for b in blobs:
				tmp=img.draw_cross(b[5], b[6])
				print("H[%d,%d]" % (b[5],b[6]))
				uart.write("H%03d,%03d" % (b[5],b[6]))
				osd[8] = b[5]
				osd[9] = b[6]
	if temp == b'r':   #黑色矩形识别初始化
		sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
		sensor.set_framesize(sensor.QQVGA)   # Set frame size to QVGA (320x240)
		sensor.set_auto_gain(True) # 自动曝光
		sensor.set_auto_whitebal(True) # 自动白平衡
		sensor.skip_frames(time = 1000)     # Wait for settings take effect.
		print("init_once")
		temp = b's'
			
	if temp == b's':   #黑色矩形识别循环
		print("rects_trace_loop")
		img = sensor.snapshot()         # Take a picture and return the image.
		blobs = img.find_rects(roi = area,threshold = 20000) #矩形下限 卡一下去掉A4以外大小的矩形
		img.draw_rectangle(area) #板卡着这个矩形放置，内部才会被识别，外部的场馆背景将被忽略
		for b in blobs:
			print(b.corners())
			print(b.magnitude())
			img.draw_circle(b.corners()[0][0], b.corners()[0][1], 5, color = (0, 255, 0))  #控制台调参
			img.draw_circle(b.corners()[1][0], b.corners()[1][1], 5, color = (0, 255, 0))  #控制台调参
			img.draw_circle(b.corners()[2][0], b.corners()[2][1], 5, color = (0, 255, 0))  #控制台调参
			img.draw_circle(b.corners()[3][0], b.corners()[3][1], 5, color = (0, 255, 0))  #控制台调参

			uart.write("A%03d,%03d" % (b.corners()[0][0],b.corners()[0][1]))
			uart.write("B%03d,%03d" % (b.corners()[1][0],b.corners()[1][1]))
			uart.write("C%03d,%03d" % (b.corners()[2][0],b.corners()[2][1]))
			uart.write("D%03d,%03d" % (b.corners()[3][0],b.corners()[3][1]))

			osd[0] = b.corners()[0][0]
			osd[1] = b.corners()[0][1]
			osd[2] = b.corners()[1][0]
			osd[3] = b.corners()[1][1]
			osd[4] = b.corners()[2][0]
			osd[5] = b.corners()[2][1]
			osd[6] = b.corners()[3][0]
			osd[7] = b.corners()[3][1]
			#print("R(%03d,%03d)(%03d,%03d)(%03d,%03d)(%03d,%03d)\r\n" % (b.corners()[0][0],b.corners()[0][1],
			#b.corners()[1][0],b.corners()[1][1],
			#b.corners()[2][0],b.corners()[2][1],
			#b.corners()[3][0],b.corners()[3][1],))

			img.draw_string(0, 0,"A%d,%d" % (osd[0],osd[1]))
			img.draw_string(0, 0+10,"B%d,%d" % (osd[2],osd[3]))
			img.draw_string(0, 0+20,"C%d,%d" % (osd[4],osd[5]))
			img.draw_string(0, 0+30,"D%d,%d" % (osd[6],osd[7]))
			img.draw_string(0, 0+40,"H%d,%d" % (osd[8],osd[9]))
			img.draw_string(0, 0+50,"L%d,%d" % (osd[10],osd[11]))

	lcd.display(img)                # Display on LCD
	print(temp)
	print(clock.fps())              # Note: MaixPy's Cam runs about half as fast when connected
	                                # to the IDE. The FPS should increase once disconnected.
